Homework 3 - ENPM 662

Hemanth Joseph Raj - UID: 117518955

# import <needed_package_name>
import sys
import math # For Math functions
import sympy as sym # For declaring variables as symbols, make sure you have sympy installed in your system
from sympy import symbols

Problem 1

We have been given a seven DOF Kuka robot. We need to solve for the forward kinematics of the robot by applying the method given by Denavit-Hartenberg.

Solution

By following the rules layed out by the D-H convention of solving the problem

1. Make the zi - axes for all frames (along the axis of actuation of joint i+1)
2. The base frame is already mentioned in the problem
3. Now we need to assign the xi - axes for all frames by checking the relation between zi-1 and zi (if they are coplanar (parallel or intersect) or not coplanar)
4. After all xi - axes are assigned, we complete the frame by attaching the yi - axes to complete the Right Handed Frame rule

The co-ordinate frames are shown as below figure

The D-H Parameters table is mentioned as below

Now we need to write the individual transformation matrices for each transformation

We know that the general form of a transformation matrix is

xi = symbols('theta_i')
yi = symbols('alpha_i')
di = symbols('d_i')
ai = symbols('a_i')
cxi = sym.cos(xi)
sxi = sym.sin(xi)
cyi = sym.cos(yi)
syi = sym.sin(yi)
T0_i = sym.Matrix([[cxi, -sxi*cyi, sxi*syi, ai*cxi], [sxi, cxi*cyi, -cxi*syi, ai*syi], [0, syi, cyi, di], [0, 0, 0, 1]])
T0_i

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{i} \right)} & - \sin{\left(\theta_{i} \right)} \cos{\left(\alpha_{i} \right)} & \sin{\left(\alpha_{i} \right)} \sin{\left(\theta_{i} \right)} & a_{i} \cos{\left(\theta_{i} \right)}\\\sin{\left(\theta_{i} \right)} & \cos{\left(\alpha_{i} \right)} \cos{\left(\theta_{i} \right)} & - \sin{\left(\alpha_{i} \right)} \cos{\left(\theta_{i} \right)} & a_{i} \sin{\left(\alpha_{i} \right)}\\0 & \sin{\left(\alpha_{i} \right)} & \cos{\left(\alpha_{i} \right)} & d_{i}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 1

x1 = symbols('x1') # Consider x1 as theta 1
d1 = symbols('d1')
c1 = sym.cos(x1)
s1 = sym.sin(x1)
T0_1 = sym.Matrix([[c1, 0, -s1, 0], [s1, 0, c1, 0], [0, -1, 0, d1], [0, 0, 0, 1]])
T0_1
#since the D-H parameter a is zero throughout, I don't mention it anywhere

$\displaystyle \left[\begin{matrix}\cos{\left(x_{1} \right)} & 0 & - \sin{\left(x_{1} \right)} & 0\\\sin{\left(x_{1} \right)} & 0 & \cos{\left(x_{1} \right)} & 0\\0 & -1 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 1 to 2

x2 = symbols('x2') # Consider x2 as theta 2
d2 = symbols('d2') # d2 = 0
c2 = sym.cos(x2)
s2 = sym.sin(x2)
T1_2 = sym.Matrix([[c2, 0, s2, 0], [s2, 0, -c2, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T1_2

$\displaystyle \left[\begin{matrix}\cos{\left(x_{2} \right)} & 0 & \sin{\left(x_{2} \right)} & 0\\\sin{\left(x_{2} \right)} & 0 & - \cos{\left(x_{2} \right)} & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 2 to 3

x3 = symbols('x3') # Consider x3 as theta 3
d3 = symbols('d3')
c3 = sym.cos(x3)
s3 = sym.sin(x3)
T2_3 = sym.Matrix([[c3, 0, s3, 0], [s3, 0, -c3, 0], [0, 1, 0, d3], [0, 0, 0, 1]])
T2_3

$\displaystyle \left[\begin{matrix}\cos{\left(x_{3} \right)} & 0 & \sin{\left(x_{3} \right)} & 0\\\sin{\left(x_{3} \right)} & 0 & - \cos{\left(x_{3} \right)} & 0\\0 & 1 & 0 & d_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 3 to 4

x4 = symbols('x4') # Consider x4 as theta 4
d4 = symbols('d4') # d4 = 0
c4 = sym.cos(x4)
s4 = sym.sin(x4)
T3_4 = sym.Matrix([[c4, 0, -s4, 0], [s4, 0, c4, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
T3_4

$\displaystyle \left[\begin{matrix}\cos{\left(x_{4} \right)} & 0 & - \sin{\left(x_{4} \right)} & 0\\\sin{\left(x_{4} \right)} & 0 & \cos{\left(x_{4} \right)} & 0\\0 & -1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 4 to 5

x5 = symbols('x5') # Consider x5 as theta 5
d5 = symbols('d5')
c5 = sym.cos(x5)
s5 = sym.sin(x5)
T4_5 = sym.Matrix([[c5, 0, -s5, 0], [s5, 0, c5, 0], [0, -1, 0, d5], [0, 0, 0, 1]])
T4_5

$\displaystyle \left[\begin{matrix}\cos{\left(x_{5} \right)} & 0 & - \sin{\left(x_{5} \right)} & 0\\\sin{\left(x_{5} \right)} & 0 & \cos{\left(x_{5} \right)} & 0\\0 & -1 & 0 & d_{5}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 5 to 6

x6 = symbols('x6') # Consider x6 as theta 6
d6 = symbols('d6') # d6 = 0
c6 = sym.cos(x6)
s6 = sym.sin(x6)
T5_6 = sym.Matrix([[c6, 0, s6, 0], [s6, 0, -c6, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T5_6

$\displaystyle \left[\begin{matrix}\cos{\left(x_{6} \right)} & 0 & \sin{\left(x_{6} \right)} & 0\\\sin{\left(x_{6} \right)} & 0 & - \cos{\left(x_{6} \right)} & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 6 to 7

x7 = symbols('x7') # Consider x7 as theta 7
d7 = symbols('d7')
c7 = sym.cos(x7)
s7 = sym.sin(x7)
T6_7 = sym.Matrix([[c7, -s7, 0, 0], [s7, c7, 0, 0], [0, 0, 1, d7], [0, 0, 0, 1]])
T6_7

$\displaystyle \left[\begin{matrix}\cos{\left(x_{7} \right)} & - \sin{\left(x_{7} \right)} & 0 & 0\\\sin{\left(x_{7} \right)} & \cos{\left(x_{7} \right)} & 0 & 0\\0 & 0 & 1 & d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]$

Now we find the total transformation matrix from o to 7

T0_7 = (T0_1)*(T1_2)*(T2_3)*(T3_4)*(T4_5)*(T5_6)*(T6_7)
T0_7

$\displaystyle \left[\begin{matrix}\left(\left(\left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \cos{\left(x_{7} \right)} + \left(- \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)}\right) \sin{\left(x_{7} \right)} & - \left(\left(\left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \sin{\left(x_{7} \right)} + \left(- \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)}\right) \cos{\left(x_{7} \right)} & \left(\left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)} & d_{3} \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} + d_{5} \left(- \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{4} \right)}\right) + d_{7} \left(\left(\left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \cos{\left(x_{3} \right)} - \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(\left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{2} \right)} \cos{\left(x_{1} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)}\right)\\\left(\left(\left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \cos{\left(x_{7} \right)} + \left(- \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \sin{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{5} \right)}\right) \sin{\left(x_{7} \right)} & - \left(\left(\left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \sin{\left(x_{7} \right)} + \left(- \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \sin{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \cos{\left(x_{5} \right)}\right) \cos{\left(x_{7} \right)} & \left(\left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)} & d_{3} \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} + d_{5} \left(- \left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) + d_{7} \left(\left(\left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \cos{\left(x_{4} \right)} + \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)}\right) \cos{\left(x_{5} \right)} + \left(- \sin{\left(x_{1} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{2} \right)} + \cos{\left(x_{1} \right)} \cos{\left(x_{3} \right)}\right) \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(\left(\sin{\left(x_{1} \right)} \cos{\left(x_{2} \right)} \cos{\left(x_{3} \right)} + \sin{\left(x_{3} \right)} \cos{\left(x_{1} \right)}\right) \sin{\left(x_{4} \right)} - \sin{\left(x_{1} \right)} \sin{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)}\right)\\\left(\left(\left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(- \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{3} \right)} - \cos{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \cos{\left(x_{7} \right)} + \left(- \left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{5} \right)}\right) \sin{\left(x_{7} \right)} & - \left(\left(\left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \sin{\left(x_{5} \right)}\right) \cos{\left(x_{6} \right)} + \left(- \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{3} \right)} - \cos{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \sin{\left(x_{6} \right)}\right) \sin{\left(x_{7} \right)} + \left(- \left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \sin{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \cos{\left(x_{5} \right)}\right) \cos{\left(x_{7} \right)} & \left(\left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(- \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{3} \right)} - \cos{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)} & d_{1} + d_{3} \cos{\left(x_{2} \right)} + d_{5} \left(\sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{3} \right)} + \cos{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) + d_{7} \left(\left(\left(- \sin{\left(x_{2} \right)} \cos{\left(x_{3} \right)} \cos{\left(x_{4} \right)} + \sin{\left(x_{4} \right)} \cos{\left(x_{2} \right)}\right) \cos{\left(x_{5} \right)} + \sin{\left(x_{2} \right)} \sin{\left(x_{3} \right)} \sin{\left(x_{5} \right)}\right) \sin{\left(x_{6} \right)} - \left(- \sin{\left(x_{2} \right)} \sin{\left(x_{4} \right)} \cos{\left(x_{3} \right)} - \cos{\left(x_{2} \right)} \cos{\left(x_{4} \right)}\right) \cos{\left(x_{6} \right)}\right)\\0 & 0 & 0 & 1\end{matrix}\right]$

Now we can validate it for five geometrically known configurations

Example 1

First lets take the link rotation angles thetas as 0,0,0,0,0,0,0 radians

Analytical calculations

TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF1 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF1

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & d_{1} + d_{3} + d_{5} + d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]$

Geometric validation

From the figure we can see that when all angles are 0 rad, the Pz co-ordinate is nothing but d1 + d3 + d5 + d7 And the orientation is a simple identity matrix (no rotation)

Example 2

Next lets take the link rotation angles thetas as $\frac{\Pi}{2}$,0,0,0,0,0,0 radians

Analytical Calculations

TF0_1 = T0_1.subs(x1,sym.pi/2)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF2 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF2

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\1 & 0 & 0 & 0\\0 & 0 & 1 & d_{1} + d_{3} + d_{5} + d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]$

Geometric validation

When the first angle (theta 1) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the pz co-ordinate is nothing but d1 + d3 + d5 + d7

Example 3

Next lets take the link rotation angles thetas as 0,0,$\frac{\Pi}{2}$,0,0,0,0 radians

Analytical Calculations

TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,sym.pi/2)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF3 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF3

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\1 & 0 & 0 & 0\\0 & 0 & 1 & d_{1} + d_{3} + d_{5} + d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]$

Geometric validation

When the third angle (theta 3) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the pz co-ordinate is nothing but d1 + d3 + d5 + d7

Example 4

Next lets take the link rotation angles thetas as 0,0,0,$\frac{\Pi}{2}$,0,0,0 radians

Analytical Calculations

TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,sym.pi/2)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF4 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF4

$\displaystyle \left[\begin{matrix}0 & 0 & -1 & - d_{5} - d_{7}\\0 & 1 & 0 & 0\\1 & 0 & 0 & d_{1} + d_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$

Geometric validation

When the fourth angle (theta 4) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the px co-ordinate is nothing but - d5 - d7 and the pz co-ordinate is d1 + d3

Example 5

Next lets take the link rotation angles thetas as 0,0,0,0,0,$\frac{\Pi}{2}$,0 radians

Analytical Calculations

TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,sym.pi/2)
TF6_7 = T6_7.subs(x7,0)
TF5 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF5

$\displaystyle \left[\begin{matrix}0 & 0 & 1 & d_{7}\\0 & 1 & 0 & 0\\-1 & 0 & 0 & d_{1} + d_{3} + d_{5}\\0 & 0 & 0 & 1\end{matrix}\right]$

Geometric validation

When the sixth angle (theta 6) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the px co-ordinate is nothing but d7 and the pz co-ordinate is d1 + d3 + d5

---

Problem 2

We have with us a 6 DOF Puma robot we need to do Forward Velocity Kinematics and determine the Jacobian matrix for the robot using two methods Geometric Method and Derivative method

Solution

1. We need to first perform the Forward Kinematics on the robot using the D-H Convention and find its respective D-H Parameters.
2. Then we use the D-H Table to find out all the frame transformations
3. The choose both the methods (Geometric and Derivative) to solve for the Jacobian Matrix

Assigning Co-ordinate frames

We use the rules set by D-H Method as below and assign co-ordinate frames

1. Make the zi - axes for all frames (along the axis of actuation of joint i+1)
2. The base frame is already mentioned in the problem
3. Now we need to assign the xi - axes for all frames by checking the relation between zi-1 and zi (if they are coplanar (parallel or intersect) or not coplanar)
4. After all xi - axes are assigned, we complete the frame by attaching the yi - axes to complete the Right Handed Frame rule

Co-ordinate Frames and D-H Parameters Table

The co-ordinate frame assignment is shown below along with the D-H Parameters table

Now we need to write the individual transformation matrices for each transformation

Transformation from 0 to 1

y1 = symbols('y1') # Consider y1 as theta 1
b1 = symbols('b1') # Using notation b instead of d
a1 = symbols('a1') # a1 = 0
ct1 = sym.cos(y1)
st1 = sym.sin(y1)
H0_1 = sym.Matrix([[ct1, 0, -st1, 0], [st1, 0, ct1, 0], [0, -1, 0, b1], [0, 0, 0, 1]])
H0_1

$\displaystyle \left[\begin{matrix}\cos{\left(y_{1} \right)} & 0 & - \sin{\left(y_{1} \right)} & 0\\\sin{\left(y_{1} \right)} & 0 & \cos{\left(y_{1} \right)} & 0\\0 & -1 & 0 & b_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 1 to 2

y2 = symbols('y2') # Consider y2 as theta 2
b2 = symbols('b2') # Using notation b instead of d, b2 (i.e d2) = 0
a2 = symbols('a2')
ct2 = sym.cos(y2)
st2 = sym.sin(y2)
H1_2 = sym.Matrix([[ct2, -st2, 0, a2*ct2], [st2, ct2, 0, a2*st2], [0, 0, 1, 0], [0, 0, 0, 1]])
H1_2

$\displaystyle \left[\begin{matrix}\cos{\left(y_{2} \right)} & - \sin{\left(y_{2} \right)} & 0 & a_{2} \cos{\left(y_{2} \right)}\\\sin{\left(y_{2} \right)} & \cos{\left(y_{2} \right)} & 0 & a_{2} \sin{\left(y_{2} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 2

H0_2 = H0_1*H1_2
H0_2

$\displaystyle \left[\begin{matrix}\cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} & - \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} & - \sin{\left(y_{1} \right)} & a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\\\sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} & - \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} & \cos{\left(y_{1} \right)} & a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\\- \sin{\left(y_{2} \right)} & - \cos{\left(y_{2} \right)} & 0 & - a_{2} \sin{\left(y_{2} \right)} + b_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 2 to 3

y3 = symbols('y3') # Consider y3 as theta 3
b3 = symbols('b3') # Using notation b instead of d
a3 = symbols('a3')
ct3 = sym.cos(y3)
st3 = sym.sin(y3)
H2_3 = sym.Matrix([[ct3, 0, -st3, a3*ct3], [st3, 0, ct3, a3*st3], [0, -1, 0, b3], [0, 0, 0, 1]])
H2_3

$\displaystyle \left[\begin{matrix}\cos{\left(y_{3} \right)} & 0 & - \sin{\left(y_{3} \right)} & a_{3} \cos{\left(y_{3} \right)}\\\sin{\left(y_{3} \right)} & 0 & \cos{\left(y_{3} \right)} & a_{3} \sin{\left(y_{3} \right)}\\0 & -1 & 0 & b_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 3

H0_3 = H0_2*H2_3
H0_3

$\displaystyle \left[\begin{matrix}- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} & \sin{\left(y_{1} \right)} & - \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} & a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)}\\- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} & - \cos{\left(y_{1} \right)} & - \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} & a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)}\\- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} & 0 & \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} & - a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 3 to 4

y4 = symbols('y4') # Consider y4 as theta 4
b4 = symbols('b4') # Using notation b instead of d
a4 = symbols('a4') #a4 = 0
ct4 = sym.cos(y4)
st4 = sym.sin(y4)
H3_4 = sym.Matrix([[ct4, 0, st4, 0], [st4, 0, -ct4, 0], [0, 1, 0, b4], [0, 0, 0, 1]])
H3_4

$\displaystyle \left[\begin{matrix}\cos{\left(y_{4} \right)} & 0 & \sin{\left(y_{4} \right)} & 0\\\sin{\left(y_{4} \right)} & 0 & - \cos{\left(y_{4} \right)} & 0\\0 & 1 & 0 & b_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 4

H0_4 = H0_3*H3_4
H0_4

$\displaystyle \left[\begin{matrix}\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)} & - \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} & \left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} - \sin{\left(y_{1} \right)} \cos{\left(y_{4} \right)} & a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)} & - \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} & \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{4} \right)} & a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\\left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{4} \right)} & \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} & \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{4} \right)} & - a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 4 to 5

y5 = symbols('y5') # Consider y5 as theta 5
b5 = symbols('b5') # Using notation b instead of d, b5 (i.e d5) = 0)
a5 = symbols('a5') # a5 = 0
ct5 = sym.cos(y5)
st5 = sym.sin(y5)
H4_5 = sym.Matrix([[ct5, 0, -st5, 0], [st5, 0, ct5, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
H4_5

$\displaystyle \left[\begin{matrix}\cos{\left(y_{5} \right)} & 0 & - \sin{\left(y_{5} \right)} & 0\\\sin{\left(y_{5} \right)} & 0 & \cos{\left(y_{5} \right)} & 0\\0 & -1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 5

H0_5 = H0_4*H4_5
H0_5

$\displaystyle \left[\begin{matrix}\left(\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)}\right) \cos{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)} & - \left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{4} \right)} & - \left(\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)} & a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\\left(\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)}\right) \cos{\left(y_{5} \right)} + \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)} & - \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} - \cos{\left(y_{1} \right)} \cos{\left(y_{4} \right)} & - \left(\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)} & a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\\left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{4} \right)} \cos{\left(y_{5} \right)} & - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{4} \right)} & \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{5} \right)} - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)} \cos{\left(y_{4} \right)} & - a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 5 to 6

y6 = symbols('y6') # Consider y1 as theta 1
b6 = symbols('b6') # Using notation b instead of d, b5 (i.e d5) = 0)
a6 = symbols('a6') # a6 = 0
ct6 = sym.cos(y6)
st6 = sym.sin(y6)
H5_6 = sym.Matrix([[ct6, 0, 0, 0], [st5, ct5, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
H5_6

$\displaystyle \left[\begin{matrix}\cos{\left(y_{6} \right)} & 0 & 0 & 0\\\sin{\left(y_{5} \right)} & \cos{\left(y_{5} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation from 0 to 6

H0_6 = H0_5*H5_6
H0_6

$\displaystyle \left[\begin{matrix}\left(\left(\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)}\right) \cos{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)}\right) \cos{\left(y_{6} \right)} + \left(- \left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\right) \sin{\left(y_{5} \right)} & \left(- \left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\right) \cos{\left(y_{5} \right)} & - \left(\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)} & a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\\left(\left(\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)}\right) \cos{\left(y_{5} \right)} + \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)}\right) \cos{\left(y_{6} \right)} + \left(- \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} - \cos{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\right) \sin{\left(y_{5} \right)} & \left(- \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} - \cos{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\right) \cos{\left(y_{5} \right)} & - \left(\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)} & a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\\left(\left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{4} \right)} \cos{\left(y_{5} \right)}\right) \cos{\left(y_{6} \right)} - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{4} \right)} \sin{\left(y_{5} \right)} & - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{4} \right)} \cos{\left(y_{5} \right)} & \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{5} \right)} - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)} \cos{\left(y_{4} \right)} & - a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\\0 & 0 & 0 & 1\end{matrix}\right]$

Now we apply the First method for solving the Jacobian

Geometry Method

We know that for each Jacobian matrix for each transformation $Ji$ = $\binom{Jv}{Jw}$ = $\binom{Z(i-1) X (O(n) - O(i-1))}{Z(i-1)}$ (For a Revolute joint)

Where $Jv$ is the Jacobian for linear velocity and $Jw$ is the Jacobian for angular velocity

And put all individual jacobians together and form the final Jacobian Matrix from o to n $$

\mathbf{J^{0}_6}

\begin{pmatrix} J1 & J2 & J3 & J4 & J5 & J6 \end{pmatrix}

$$

So now we find the following

1. vectors O1, O2, O3, O4, O5 and O6 (i.e On)
2. $r_i$ where i = 1 to n = $On - O(i-1)$
3. vectors z0, z1, z2, z3, z4, z5

On = sym.Matrix(H0_6.col(3)[0:3])
On

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

O0 = sym.Matrix([0, 0, 0])
O0

$\displaystyle \left[\begin{matrix}0\\0\\0\end{matrix}\right]$

O1 = sym.Matrix(H0_1.col(3)[:3])
O1

$\displaystyle \left[\begin{matrix}0\\0\\b_{1}\end{matrix}\right]$

O2 = sym.Matrix(H0_2.col(3)[:3])
O2

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\\- a_{2} \sin{\left(y_{2} \right)} + b_{1}\end{matrix}\right]$

O3 = sym.Matrix(H0_3.col(3)[:3])
O3

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)}\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)}\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1}\end{matrix}\right]$

O4 = sym.Matrix(H0_4.col(3)[:3])
O4

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

O5 = sym.Matrix(H0_5.col(3)[:3])
O5

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

r1 = On-O0
r1

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{1} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

r2 = On-O1
r2

$\displaystyle \left[\begin{matrix}a_{2} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\a_{2} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} - a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{2} \sin{\left(y_{2} \right)} - a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

r3 = On-O2
r3

$\displaystyle \left[\begin{matrix}- a_{3} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + a_{3} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - b_{3} \sin{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{3} \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + a_{3} \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)} + b_{3} \cos{\left(y_{1} \right)} + b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\- a_{3} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - a_{3} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)} + b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

r4 = On-O3
r4

$\displaystyle \left[\begin{matrix}b_{4} \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right)\\b_{4} \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right)\\b_{4} \left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right)\end{matrix}\right]$

r5 = On-O4
r5

$\displaystyle \left[\begin{matrix}0\\0\\0\end{matrix}\right]$

r6 = On-O5
r6

$\displaystyle \left[\begin{matrix}0\\0\\0\end{matrix}\right]$

z0 = sym.Matrix([0, 0, 1])
z0

$\displaystyle \left[\begin{matrix}0\\0\\1\end{matrix}\right]$

z1 = sym.Matrix(H0_1.col(2)[:3])
z1

$\displaystyle \left[\begin{matrix}- \sin{\left(y_{1} \right)}\\\cos{\left(y_{1} \right)}\\0\end{matrix}\right]$

z2 = sym.Matrix(H0_2.col(2)[:3])
z2

$\displaystyle \left[\begin{matrix}- \sin{\left(y_{1} \right)}\\\cos{\left(y_{1} \right)}\\0\end{matrix}\right]$

z3 = sym.Matrix(H0_3.col(2)[:3])
z3

$\displaystyle \left[\begin{matrix}- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\\- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\\\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\end{matrix}\right]$

z4 = sym.Matrix(H0_4.col(2)[:3])
z4

$\displaystyle \left[\begin{matrix}\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} - \sin{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\\\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \sin{\left(y_{4} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{4} \right)}\\\left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{4} \right)}\end{matrix}\right]$

z5 = sym.Matrix(H0_5.col(2)[:3])
z5

$\displaystyle \left[\begin{matrix}- \left(\left(- \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} + \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} + \sin{\left(y_{1} \right)} \sin{\left(y_{4} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{1} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)}\\- \left(\left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} + \sin{\left(y_{1} \right)} \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{4} \right)} - \sin{\left(y_{4} \right)} \cos{\left(y_{1} \right)}\right) \sin{\left(y_{5} \right)} + \left(- \sin{\left(y_{1} \right)} \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{1} \right)} \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \cos{\left(y_{5} \right)}\\\left(\sin{\left(y_{2} \right)} \sin{\left(y_{3} \right)} - \cos{\left(y_{2} \right)} \cos{\left(y_{3} \right)}\right) \cos{\left(y_{5} \right)} - \left(- \sin{\left(y_{2} \right)} \cos{\left(y_{3} \right)} - \sin{\left(y_{3} \right)} \cos{\left(y_{2} \right)}\right) \sin{\left(y_{5} \right)} \cos{\left(y_{4} \right)}\end{matrix}\right]$

Now we compute the cross product of $Z(i-1)$ and $ri$ and complete the following Jacobian Matrix for each transformation $Ji$ = $\binom{Jv}{Jw}$ = $\binom{Z(i-1) X ri}{Z(i-1)}$

Now take all the $Ji$ column vectors and put them together as below to obtain the final Jacobian Matrix $J^{0}_6$ = $(J1, J2, J3, J4, J5, J6)$

Thus the Jacobian (Forward Velocity Kinematics) of the robot is obtained through method 1

Now we apply the Second method for solving the Jacobian

Derivative Method

1. From the Final transformation Matrix, isolate the Position matrix $P^{0}_6$
2. This Position Matrix gives the position of the end effector with respect to base frame, is actually a function of all the individiual joint variables $q_1$, $q_2$, $q_3$, $q_4$, $q_5$, $q_6$ Which are nothing but functions of $\theta_1$, $\theta_2$, $\theta_3$, $\theta_4$, $\theta_5$, $\theta_6$ respectively
3. Therefore $P^{0}_6$ = $x_p$ = h($q_1$, $q_2$, $q_3$, $q_4$, $q_5$, $q_6$)
4. We perform the partial derivative of the $x_p$ each time with respect to $q_i$ (i = 1 to 6)
5. We find the following partial derivatives $\displaystyle \frac{\partial x_p}{\partial q_1}$, $\displaystyle \frac{\partial x_p}{\partial q_2}$, $\displaystyle \frac{\partial x_p}{\partial q_3}$, $\displaystyle \frac{\partial x_p}{\partial q_4}$, $\displaystyle \frac{\partial x_p}{\partial q_5}$, $\displaystyle \frac{\partial x_p}{\partial q_6}$
6. Now the Jacobian Matrix is calculated as following $$

   \mathbf{J}

   \begin{pmatrix} \displaystyle \frac{\partial x_p}{\partial q_1} & \displaystyle \frac{\partial x_p}{\partial q_2} & \displaystyle \frac{\partial x_p}{\partial q_3} & \displaystyle \frac{\partial x_p}{\partial q_4} & \displaystyle \frac{\partial x_p}{\partial q_5} & \displaystyle \frac{\partial x_p}{\partial q_6}\\ z_0 & z_1 & z_2 & z_3 & z_4 & z_5 \end{pmatrix}
   $$

Thus the Jacobian (Forward Velocity Kinematics) of the robot is obtained through method 2

Comparing both the methods

1. We can see that the bottom components $z_0$ to $z_5$ for both the Jacobians is same
2. The columns $\displaystyle \frac{\partial x_p}{\partial q_4}$, $\displaystyle \frac{\partial x_p}{\partial q_5}$, $\displaystyle \frac{\partial x_p}{\partial q_6}$ are zero in second method which equates to the same in the first method where z3 X r4, z4 X r5 and z5 X r6 are all zeros which is same in both the Jacobians
3. After calculations we can see that $\displaystyle \frac{\partial x_p}{\partial q_1}$, $\displaystyle \frac{\partial x_p}{\partial q_2}$, $\displaystyle \frac{\partial x_p}{\partial q_3}$ in second matrix is same as z0 X r1, z1 X r2 and z3 X r3 in the first jacobian
4. Hence we get the same values in both methods